#include "Scene.h"

void SceneObject::Init( vector<Vec3f>& vertexList, vector<Vec3i>& faceVertexIndexList, Type type )
{
    this->ObjType = type;
    this->Mesh.SetModelData(vertexList, faceVertexIndexList);

    vector<Vec3f> triCenter;
    Int32 faceNum = this->Mesh.GetFaceNum();
    Vec3f center;
    FOR(F,0,faceNum) {
        ModelFace& face = this->Mesh.GetFace(F);
        center.x = center.y = center.z = 0.f;
        FOR(V,0,3) center += face.GetVertex(V);
        center /= 3;
        triCenter.push_back(center);
    }
    this->BVH.Build(triCenter);

    this->CollisionFlag = vector<Boolean>(faceNum, FALSE);
}

void SceneObject::Init( TriMesh& mesh, Type type )
{
    this->ObjType = type;
    this->Mesh = mesh;

    vector<Vec3f> triCenter;
    Int32 faceNum = this->Mesh.GetFaceNum();
    Vec3f center;
    FOR(F,0,faceNum) {
        ModelFace& face = this->Mesh.GetFace(F);
        center.x = center.y = center.z = 0.f;
        FOR(V,0,3) center += face.GetVertex(V);
        center /= 3;
        triCenter.push_back(center);
    }
    this->BVH.Build(triCenter);

    this->CollisionFlag = vector<Boolean>(faceNum, FALSE);
}

void SceneObject::Fit()
{
    this->BVH.TraversalBegin(BVTree::Postorder);

    BVNode* node_ptr = this->BVH.TraversalNext();
    Int32 faceIndex;
    Vec3f boundMin, boundMax, boundMin2, boundMax2, vertex;
    BVNode *lChild, *rChild;
    while(node_ptr != NULL) {
        if(this->BVH.IsLeafNode(node_ptr)) {
            faceIndex = this->BVH.GetLeafElemIndex(node_ptr);
            ModelFace& face = this->Mesh.GetFace(faceIndex);
            boundMin = boundMax = face.GetVertex(0);
            vertex = face.GetVertex(1);
            FOR(axis,0,3) {
                boundMax[axis] = max(boundMax[axis],vertex[axis]);
                boundMin[axis] = min(boundMin[axis],vertex[axis]);
            }
            vertex = face.GetVertex(2);
            FOR(axis,0,3) {
                boundMax[axis] = max(boundMax[axis],vertex[axis]);
                boundMin[axis] = min(boundMin[axis],vertex[axis]);
            }
            FOR(axis,0,3) {
                boundMax[axis] += math::EPS_F;
                boundMin[axis] -= math::EPS_F;
            }
            node_ptr->UpdateBV(boundMax, boundMin, BV_AABB::MaxMinConstruction);
        } else {
            lChild = this->BVH.GetLeftChild(node_ptr);
            rChild = this->BVH.GetRightChild(node_ptr);
            if(lChild != NULL) {
                boundMax = lChild->GetBV().GetMax();
                boundMin = lChild->GetBV().GetMin();
                if(rChild != NULL) {
                    boundMax2 = rChild->GetBV().GetMax();
                    boundMin2 = rChild->GetBV().GetMin();
                    FOR(axis,0,3) {
                        boundMax[axis] = max(boundMax[axis], boundMax2[axis]);
                        boundMin[axis] = min(boundMin[axis], boundMin2[axis]);
                    }
                }
                node_ptr->UpdateBV(boundMax, boundMin, BV_AABB::MaxMinConstruction);
            } else { // rChild != NULL
                boundMax = rChild->GetBV().GetMax();
                boundMin = rChild->GetBV().GetMin();
                node_ptr->UpdateBV(boundMax, boundMin, BV_AABB::MaxMinConstruction);
            }
        }
        node_ptr = this->BVH.TraversalNext();
    }

    this->BVH.TraversalEnd();
}

void SceneObject::CollisionCheck( SceneObject& another )
{
    fill(CollisionFlag.begin(),CollisionFlag.end(),FALSE);
    _firstObj  = this;
    _secondObj = &another;
    collisionCheck(this->BVH.GetRoot(), another.BVH.GetRoot());
}

void SceneObject::SelfCollisionCheck()
{
    fill(CollisionFlag.begin(),CollisionFlag.end(),FALSE);
    _firstObj = _secondObj = this;
    selfCollisionCheck(this->BVH.GetRoot());
}

SceneObject* SceneObject::_firstObj  = NULL;
SceneObject* SceneObject::_secondObj = NULL;

void SceneObject::collisionCheck( BVNode* firstNode, BVNode* secondNode )
{
    BOOST_ASSERT(_firstObj != NULL);
    BOOST_ASSERT(_secondObj != NULL);

    if( !BVNode::CheckBVIntersection(firstNode,secondNode) ) return;

    TriMesh& firstMesh  = _firstObj->Mesh;
    TriMesh& secondMesh = _secondObj->Mesh;
    BVTree& firstTree   = _firstObj->BVH;
    BVTree& secondTree  = _secondObj->BVH;
    Boolean firstLeaf  = firstTree.IsLeafNode(firstNode);
    Boolean secondLeaf = secondTree.IsLeafNode(secondNode);
    BVNode *firstLchild, *firstRchild;
    BVNode *secondLchild, *secondRchild;
    if(firstLeaf) {
        if(secondLeaf) {
            Int32 firstFaceId  = firstTree.GetLeafElemIndex(firstNode);
            Int32 secondFaceId = secondTree.GetLeafElemIndex(secondNode);
            ModelFace& firstFace  = firstMesh.GetFace(firstFaceId);
            ModelFace& secondFace = secondMesh.GetFace(secondFaceId);
            if(!firstFace.IsNeighbour(secondFace)) {
                if( TriPairTester::TestStatic(firstFace.GetTriangle(), secondFace.GetTriangle()) ) {
                    _firstObj->CollisionFlag[firstFaceId]   = TRUE;
                    _secondObj->CollisionFlag[secondFaceId] = TRUE;
                }
            }
        } else {
            secondLchild = secondTree.GetLeftChild(secondNode);
            secondRchild = secondTree.GetRightChild(secondNode);
            if(secondLchild != NULL)
                collisionCheck(firstNode, secondLchild);
            if(secondRchild != NULL)
                collisionCheck(firstNode, secondRchild);
        }
    } else {
        firstLchild = firstTree.GetLeftChild(firstNode);
        firstRchild = firstTree.GetRightChild(firstNode);
        if(secondLeaf) {
            if(firstLchild != NULL)
                collisionCheck(firstLchild, secondNode);
            if(firstRchild != NULL)
                collisionCheck(firstRchild, secondNode);
        } else {
            secondLchild = secondTree.GetLeftChild(secondNode);
            secondRchild = secondTree.GetRightChild(secondNode);
            if(firstLchild != NULL) {
                if(secondLchild != NULL) collisionCheck(firstLchild, secondLchild);
                if(secondRchild != NULL) collisionCheck(firstLchild, secondRchild);
            }
            if(firstRchild != NULL) {
                if(secondLchild != NULL) collisionCheck(firstRchild, secondLchild);
                if(secondRchild != NULL) collisionCheck(firstRchild, secondRchild);
            }
        }
    }
}

void SceneObject::selfCollisionCheck( BVNode* node )
{
    BOOST_ASSERT(node != NULL);

    BVTree& tree = _firstObj->BVH;
    BVNode* leftChild  = tree.GetLeftChild(node);
    BVNode* rightChild = tree.GetRightChild(node);

    if(leftChild != NULL && rightChild != NULL)
        collisionCheck(leftChild, rightChild);
    if(leftChild != NULL)
        selfCollisionCheck(leftChild);
    if(rightChild != NULL)
        selfCollisionCheck(rightChild);
}

